path(path,'../libs/publicMatlabLibs/');
cl
datafileFolder = '../data/simResults/parameterChange/';
resultFolder   = '../data/ECI/';
for satnum =  1:1
    load(strcat(datafileFolder,'sat',num2str(satnum),'.mat'));
    load(strcat(resultFolder,num2str(satnum),'.mat'));
    kineTrueState = kineTrueState';
    timeTrueTagsRelative  = (timeTrueTags'-timeTrueTags(1))/3600;
    kineStateCovslength = size(kineStateCovs,3);
    
    startprint = 1;
    endprint =  kineStateCovslength;
%     endprint =100;
    timeTagsRelative  = (timeTags'-timeTags(1))/3600;
    trs       = traceSeries(kineStateCovs(1:3,1:3,startprint:endprint));
    figure(satnum);
    plot(timeTagsRelative(startprint:endprint),sqrt(trs),'r+'); 
    hold on 
    plot(timeTagsRelative(startprint:endprint),sqrt(trs));
    figure()
    plot(timeTagsRelative,kineStateMeans(1,:))
    hold on 
    plot(timeTrueTagsRelative,kineTrueState(1,:));
    pickedKinePoints =zeros(6,1);
    tmpIndex =1;
    pickedindices    = zeros(1);
    while true
        if tmpIndex > length(timeTrueTags)
            break
        end
        [m,index] = min(abs(timeTags - timeTrueTags(tmpIndex)));
        if abs(m)< 1e-4
                pickedKinePoints(:,tmpIndex) =  kineStateMeans(:,index);
                pickedindices(tmpIndex) = index;
                tmpIndex  =  tmpIndex +1;
%                 disp(index);
        else
            break;
        end
    end
    tmpIndex  =500;
%     tmpIndex  =  tmpIndex -1;
    rmse = (pickedKinePoints(1:3,1:tmpIndex) -kineTrueState(1:3,1:tmpIndex));
    rmsev = (pickedKinePoints(4:6,1:tmpIndex) -kineTrueState(4:6,1:tmpIndex));
    figure();
    plot(timeTrueTagsRelative(1:tmpIndex),rmse);
    figure();
    plot(timeTrueTagsRelative(1:tmpIndex),sqrt(sum(rmse.^2,1)));
    figure();
    plot(timeTrueTagsRelative(1:tmpIndex),rmsev);
end
% figure()
% hist(sqrt(trs),100);